#!/usr/bin/env python  
"""
My solution for Assignment 2, Part 2, Task 1 in COMP 4766, Winter 2014.

Name: Daniel Cook
Email: dac456@mun.ca
Student number: 
"""

from geometry_msgs.msg import Twist
import math

class SmoothController1:
    def __init__(self, goalIx, goalIy):
        """
        Initialize the class with the position of the goal in the robot ref.
        frame (goalIx, goalIy).
        """
        self.goalIx = goalIx
        self.goalIy = goalIy
        self.goalRx = self.dot_product(math.cos(0), math.sin(0), goalIx, goalIy)
        self.goalRy = self.dot_product(-math.sin(0), math.cos(0), goalIx, goalIy)
  
    """
    Helper method for calculating dot product (for coordinate transformation)
    """
    def dot_product(self, x1, y1, x2, y2):
        return (x1*x2)+(y1*y2)

    def get_twist(self, x, y, theta):
        """
        Given the current pose of the robot (x, y, theta) compute and return
        the appropriate Twist message.
        """
        t = Twist()
        t.linear.x = 0
        t.angular.z = 0
        
        v = 1*self.goalRx
        w = 0.75*self.goalRy
        
        if(self.goalRx != 0):
            t.linear.x = v
            
        if(self.goalRy >= 0):
            t.angular.z = -w
        else:
            t.angular.z = w
            
        gdx = self.goalIx - x
        gdy = self.goalIy - y
        
        self.goalRx = self.dot_product(math.cos(theta), math.sin(theta), gdx, gdy)
        self.goalRy = self.dot_product(-math.sin(theta), math.cos(theta), gdx, gdy)  
        
        return t
